#include <unistd.h>//Linux系统下网络通讯的头文件集合
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <malloc.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <stdarg.h>
#include <fcntl.h>
#include <iostream>
#include <cmath>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include <stdint.h>
#include <sys/stat.h>
#include <sys/types.h>
#include "../include/car_odom_upload.h"
using namespace std;


struct Car_to_Nano
{
    float current_speed;
    float current_angular_velocity;

};


int m_sockClient;
sockaddr_in m_servaddr;
int m_sockClient_s;
sockaddr_in m_servaddr_s;

float steer_angle = 0.0;


/////////////////////////////////cesu/////////////////////
/* eQEP interface */
struct tieqep_mparams {
	uint32_t sampling_freq;
};

struct tieqep_minfo {
	uint32_t direction;
	uint32_t pulse_numbers;
};

struct m_speed {
    uint32_t direction;
    double rpm;
};

struct eqep_params {
	uint32_t sampling_freq;
    double encoder_lines;
};

struct eqep_handle {
    int32_t fd;
    struct eqep_params eqep_params;
};

#define TIEQEP_IOC_SET_MPARAMS	(0x10001)
#define TIEQEP_IOC_GET_MPARAMS	(0x10002)
#define TIEQEP_IOC_GET_MINFO	(0x10003)

int32_t eqep_open(struct eqep_handle *eqep_handle, const char *dev_name)
{
    eqep_handle->fd = open(dev_name, O_RDWR);
    if (eqep_handle->fd < 0) {
        perror("open error");
        return -1;
    }

    return 0;
}

int32_t eqep_close(struct eqep_handle *eqep_handle)
{
    return close(eqep_handle->fd);
}

int32_t eqep_config(struct eqep_handle *eqep_handle, struct eqep_params *eqep_params)
{
    int32_t ret = 0;
    struct tieqep_mparams mparams;

    mparams.sampling_freq = eqep_params->sampling_freq;
	ret = ioctl(eqep_handle->fd, TIEQEP_IOC_SET_MPARAMS, &mparams);
	if (ret < 0) {
        perror("ioctl error");
		return -1;
	}

    memcpy(&eqep_handle->eqep_params, eqep_params, sizeof(struct eqep_params));

    return ret;
}

int32_t eqep_get_m_speed(struct eqep_handle *eqep_handle, struct m_speed *m_speed)
{
    int32_t ret = 0;
    struct tieqep_minfo minfo;
    uint32_t direction = 0;
    double rpm = 0.0;

    ret = ioctl(eqep_handle->fd, TIEQEP_IOC_GET_MINFO, &minfo);
    rpm = (15*eqep_handle->eqep_params.sampling_freq*minfo.pulse_numbers)/eqep_handle->eqep_params.encoder_lines;
    m_speed->rpm = rpm;
    m_speed->direction = minfo.direction;
    printf("pulse_numbers %d\n", minfo.pulse_numbers);

    return ret;
}
/////////////////////odom/////////////////////////
void odom_upload(int s)
{
    printf ("start get odom data:\n");
//--------------rqep get speed------------------//
	float steer_angle;
	int32_t ret = 0;
	struct eqep_handle eqep_handle;
	struct eqep_params eqep_params;
    struct m_speed m_speed;
	float r = 0.034;
	float pi = 3.141592654;
	float current_speed = 0.0;
	float current_angular_velocity = 0.0;
	float wheelbase = 0.168;

	extern int m_sockClient_s;
	extern sockaddr_in m_servaddr_s;
	if ((m_sockClient_s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)    //创建socket句柄，采用UDP协议
	{
		printf("create socket error: %s(errno: %d)\n", strerror(errno), errno);
	}

	memset(&m_servaddr_s, 0, sizeof(m_servaddr_s));  //初始化结构体
	m_servaddr_s.sin_family = AF_INET;           //设置通信方式
	m_servaddr_s.sin_port = htons(0x8888);         //设置端口号
	m_servaddr_s.sin_addr.s_addr = inet_addr("192.168.2.1");

	bind(m_sockClient_s, (sockaddr*)&m_servaddr_s, sizeof(m_servaddr_s));//绑定套接字

	Car_to_Nano odom_data;
	memset((void*)&odom_data, 0, sizeof(Car_to_Nano));

	ret = eqep_open(&eqep_handle, "/dev/eqep-2");
    	if (ret < 0) {
            printf("eqep_open error\n");
    	}

	eqep_params.encoder_lines = 224.5;
	eqep_params.sampling_freq = 50;
	ret = eqep_config(&eqep_handle, &eqep_params);
	if (ret < 0) {
	    printf("eqep_config error\n");
	}
	while(1)
	{
	    ret = eqep_get_m_speed(&eqep_handle, &m_speed);

	    if (ret < 0) {
	        if (errno == EAGAIN) {
	     	    usleep(20000);
		    continue;
		    } else {
		        printf("eqep_get_m_speed error\n");
		    }
	    }

	    if(m_speed.direction == 1)
	    {
	        current_speed = (m_speed.rpm*2*pi*r)/60;
	    }else
	    {	
	        current_speed = -(m_speed.rpm*2*pi*r)/60;
	    }
	    printf("current_speed %f\n", current_speed);
	    //pthread_mutex_lock(&mut);
	    steer_angle = -0.064017*s;//(xtc.cervo_command);
	    current_angular_velocity = current_speed*tan(steer_angle)/wheelbase;//角速度
	    printf("current_angular_velocity %f\n", current_angular_velocity);

	    odom_data.current_speed = current_speed;
	    odom_data.current_angular_velocity = current_angular_velocity;

	    int send = sendto(m_sockClient_s, (void*)&odom_data, sizeof(Car_to_Nano), 0, (const sockaddr*)& m_servaddr_s, sizeof(m_servaddr_s));
	    cout << "odom send: " <<send << endl;

	    //pthread_mutex_unlock(&mut);
	}
	eqep_close(&eqep_handle);
}


